#include "MTorus.h"

float cubicRoot(float x) {
	return x<0? -pow(-x, float(1.0/3.0)): pow(x, float(1.0/3.0));
}

MTorus::MTorus(float ar1, float ar2, int ars, int avs, float aox, float aoy,float aoz)
{
	name = "Torus";
	verts = NULL;
	normal = NULL;
	bNormal = false;
	updateTorus(ar1,ar2,ars,avs,aox,aoy,aoz);
}


MTorus::~MTorus(void)
{
	if(verts) {
		for(int i = 0; i<rs; i++) {
			delete []verts[i];
		}
		delete []verts;
	}
	if(normal) {
		for(int i = 0; i<rs; i++) {
			delete normal[i];
		}
		delete []normal;
	}
}

MTorus *MTorus::makeTorus(float ar1, float ar2, int ars, int avs, float aox, float aoy,float aoz) {
	if(ar1<=0 || ar2==0 || ar1<ar2 || ars < 3 || avs < 3) {//at least 1 radial stacks and 4 vertical slice
		return NULL;
	}
	return new MTorus(ar1,ar2,ars,avs,aox,aoy,aoz);
}

void MTorus::drawShape(void(*func)(const MVertex &p1,  const MVertex &p2, const MVertex &p3, int ic),
	void(*drawline)(const MVertex &p1,  const MVertex &p2,int ic)) {
		drawTorus(func, drawline);
}

void MTorus::drawTorus(void(*func)(const MVertex &p1,  const MVertex &p2, const MVertex &p3, int ic),
	void(*drawline)(const MVertex &p1,  const MVertex &p2,int ic)) {
	static const int icolor = 5;
	if(!func) {	//return function
		return;
	}
	int i,j;
	for(i = 0; i<rs; i++) {
		int nextr = ((i==(rs-1))?0:i+1);
		for(j=0; j<vs; j++) {
			int nextc = ((j==(vs-1))?0:j+1);
			func(verts[i][j],verts[nextr][nextc],verts[nextr][j],icolor);
			func(verts[i][j],verts[i][nextc],verts[nextr][nextc],icolor);
		}
	}
	if(bNormal && drawline) {
		for(i=0; i<rs; i++) {
			for(j=0; j<vs; j++) {
				drawline(verts[i][j],verts[i][j]+normal[i][j]*NORMALEN,1);
			}
		}
	}
	if(step < rs) {
		step++;
	}
}

void MTorus::updateTorus(float ar1, float ar2, int ars, int avs, float aox, float aoy,float aoz) {
	if(ar1<=0 || ar2==0 || ar1<ar2 || ars < 3 || avs < 3) {//at least 1 radial stacks and 4 vertical slice
		return;
	}
	//delete old array
	if(verts) {
		for(int i = 0; i<rs; i++) {
			delete []verts[i];
		}
		delete []verts;
	}
	if(normal) {
		for(int i = 0; i<rs; i++) {
			delete normal[i];
		}
		delete []normal;
	}
	ox = aox;
	oy = aoy;
	oz = aoz;
	r1 = ar1;
	r2 = ar2;
	rs = ars;
	vs = avs;
	step = 1;
	//create array
	verts = new MVertex*[rs];
	normal = new MVertex*[rs];
	int i, j;
	for(i = 0; i<rs; i++) {
		verts[i] = new MVertex[vs];
		normal[i] = new MVertex[vs];
	}

	//populate vertexpoint
	MVertex p1(0,r2,0);
	for(j=0; j<vs; j++) {
		verts[0][j] = p1.rotateZ(2*PI/vs*(j)).translate(r1,0,0);
	}

	for(i = 1; i<rs; i++) {
		float theta = 2*PI/rs;
		for(j=0; j<vs; j++) {
			verts[i][j] = verts[i-1][j].rotateY(theta);
		}
	}

	for(i=0; i<rs; i++) {
		for(j=0; j<vs; j++) {
			MVertex top = (i==0?verts[rs-1][j]:verts[i-1][j]);
			MVertex bottom = (i==(rs-1)?verts[0][j]:verts[i+1][j]);
			MVertex left = (j==0?verts[i][vs-1]:verts[i][j-1]);
			MVertex right = (j==(vs-1)?verts[i][0]:verts[i][j+1]);
			calculateNormal(&normal[i][j],verts[i][j],top,bottom,left,right);
		}
	}

	//update own axis
	v1 = MVertex(0,-r2,0).translate(aox,aoy,aoz);
	v2 = MVertex(0,r2,0).translate(aox,aoy,aoz);

	this->rotate(90,1,0,0);
}

bool MTorus::isComplete() const {
	return step == rs;
}

void MTorus::resetStep() {
	step = 1;
}

void MTorus::incrementSlice() {
	if(vs < 50) {
		this->updateTorus(r1,r2,rs,vs+1,ox,oy,oz);
	}
}

void MTorus::decrementSlice() {
	if(vs > 3) {
		this->updateTorus(r1,r2,rs,vs-1,ox,oy,oz);
	}
}

void MTorus::incrementStack() {
	if(rs < 50) {
		this->updateTorus(r1,r2,rs+1,vs,ox,oy,oz);
	}
}

void MTorus::decrementStack() {
	if(rs > 3) {
		this->updateTorus(r1,r2,rs-1,vs,ox,oy,oz);
	}
}

void MTorus::translate(float x, float y, float z) {
	if(verts) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].translate(x,y,z);
			}
		}
		v1 = v1.translate(x,y,z);
		v2 = v2.translate(x,y,z);
	}
}

void MTorus::rotate(float deg, int x, int y, int z) {
	float theta = deg/180*PI;
	if(x==1) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].rotateX(theta);
				normal[i][j] = normal[i][j].rotateX(theta);
			}
		}
		v1 = v1.rotateX(theta);
		v2 = v2.rotateX(theta);
	}
	if(y==1) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].rotateY(theta);
				normal[i][j] = normal[i][j].rotateY(theta);
			}
		}
		v1 = v1.rotateY(theta);
		v2 = v2.rotateY(theta);
	}
	if(z==1) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].rotateZ(theta);
				normal[i][j] = normal[i][j].rotateZ(theta);
			}
		}
		v1 = v1.rotateZ(theta);
		v2 = v2.rotateZ(theta);
	}
}

void MTorus::scale(float x, float y, float z){
	if(verts) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].scaling(x,y,z);
			}
		}
		v1 = v1.scaling(x,y,z);
		v2 = v2.scaling(x,y,z);
	}
}

void MTorus::rotateAroundOwnAxis(float theta) {
	MMatrix mT, mM, mR, mMb, mTb;
	//set translate matrix
	mT.setMatrix(1,0,0,-v1.point[0],0,1,0,-v1.point[1],0,0,1,-v1.point[2],0,0,0,1);
	//set translate back matrix
	mTb.setMatrix(1,0,0,v1.point[0],0,1,0,v1.point[1],0,0,1,v1.point[2],0,0,0,1);
	//get its own world axis
	MVertex u = getVertex(v1,v2).normalize(); //u match to +x
	MVertex n,v;
	if(isEqual(u.point[0],0) && isEqual(u.point[2],0)) {//if u is the same as up vector
		if(isEqual(u.point[1],1)) {
			n = MVertex(1,0,0);
			v = MVertex(0,0,1);
		}
		else if(isEqual(u.point[1],-1)) {
			n = MVertex(0,0,1);
			v = MVertex(1,0,0);
		}
		else {
			fprintf(stderr,"rotateAroundOwnAxis error\n");
		}
	}
	else {
		n = u.CrossProduct(MVertex(0,1,0)).normalize(); //n match to +z
		v = n.CrossProduct(u).normalize(); //v match to +y
	}
	//set rotation matrix turn vertex from own axis to world axis
	mM.setMatrix(u.point[0], u.point[1], u.point[2], 0,
		v.point[0], v.point[1], v.point[2], 0,
		n.point[0], n.point[1], n.point[2],0,
		0,0,0,1);
	//set rotation matrix turn vertex from world axis to its own world
	mMb.setMatrix(u.point[0], v.point[0], n.point[0], 0,
		u.point[1], v.point[1], n.point[1], 0,
		u.point[2], v.point[2], n.point[2], 0,
		0,0,0,1);
	//set rotation matrix rotate vertex around x axis which is the central axis
	mR.setMatrix(1,0,0,0,
		0,cos(theta),-sin(theta),0,
		0,sin(theta),cos(theta),0,
		0,0,0,1);

	//now, get the final magic matrix
	//matrix = mTb * mMb * mR * mM * mT
	MMatrix normalRotation = mMb.dotMatrix(mR).dotMatrix(mM);
	matrix = mTb.dotMatrix(normalRotation).dotMatrix(mT);
	if(verts && normal) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = matrix.dotVertex(verts[i][j]);
				normal[i][j] = normalRotation.dotVertex(normal[i][j]);
			}
		}
	}
}

void MTorus::intersect(const MVertex &from, const MVertex &to) {
	MVertex p = from;
	MVertex d = (to - from).normalize();
	/*
	p = MVertex(0,2,2);
	d = MVertex(0,-1,-1);
	r1 = 2;
	r2 = 1;*/
	//inverse translate, rotate and scale
	p = p.translate(-mt.point[0], -mt.point[1], -mt.point[2]);
	p = p.rotateZ(-mr.point[2]); 
	p = p.rotateY(-mr.point[1]);
	p = p.rotateX(-mr.point[0]);
	p = p.scaling(1/ms.point[0], 1/ms.point[1], 1/ms.point[2]);
	d = d.rotateZ(-mr.point[2]);
	d = d.rotateY(-mr.point[1]);
	d = d.rotateX(-mr.point[0]);
	d = d.scaling(1/ms.point[0], 1/ms.point[1], 1/ms.point[2]);
	d = d.normalize();
	list_intersect.clear();
	//find intersection points, solve quartic equation
	//www.emeyex.com/site/projects/raytorus.pdf
	//wiki quartic equation
	//this is very complex s$%t
	//step 1. get torus equation of t
	//http://www.emeyex.com/site/projects/raytorus.pdf
	//(x^2+y^2+z^2-r^2-R^2)^2 + 4*R^2(z^2-r^2) = 0
	float c1, c2 ,c3;
	c1 = d*d;
	c2 = 2*(p*d);
	c3 = (p*p) - r2*r2 + r1*r1;
	
	float A = c1*c1;
	float B = (2 * c1 *c2)/A;
	float C = (c2*c2 + 2*c1*c3 - 4*r1*r1*(d.x()*d.x()+d.y()*d.y()))/A;
	float D = (2*c2*c3 - 8*r1*r1*(p.x()*d.x()+p.y()*d.y()))/A;
	float E = (c3*c3 - 4*r1*r1*(p.x()*p.x()+p.y()*p.y()))/A;
	A = 1;
	/*
	float A = 1;
	float B = 2*c2;
	float C = (c2*c2 + 2*c3 + pow(2*r1*d.z(),2));
	float D = (2*c2*c3 + 8*r1*r1*p.z()*d.z());
	float E = c3*c3 + 4*r1*r1*(p.z()*p.z() - r2*r2);*/
	//step 2. solve the quartic equation
	//http://www.1728.org/quartic2.htm
	//http://www.1728.org/cubic2.htm
	float t1, t2, t3, t4; //4 potential root
	float qf = C - (3*B*B/8);
	float qg = D + (pow(B,3)/8) - (B*C/2);
	float qh = E - (3*pow(B/4, 4)) + B*B*C/16 - (B*D/4);
	float cc1 = qf/2;	//cubic equation coefficient 1
	float cc2 = (qf*qf-4*qh)/16;
	float cc3 = -qg*qg/64;
	//solve cubic equation
	int numSolution = 0;
	float y1,y2,y3;
	float i2, i3;//imagin part of y2 and y3
	float cf = (3*cc2-cc1*cc1)/3;
	float cg = (2*cc1*cc1*cc1 - 9*cc1*cc2 + 27*cc3)/27;
	float ch = pow(cg/2,2) + pow(cf/3, 3);
	if(ch>0) {//only one real root
		float cR = -(cg/2) + sqrt(ch);
		float cS = cubicRoot(cR);
		float cT = -(cg/2) - sqrt(ch);
		float cU = cubicRoot(cT);
		y1 = cU +  cS - cc1/3;
		y2 = - (cS+cU)/2 - cc1/3;
		i2 = (cS-cU)*sqrt(3.0)/2;
		y3 = - y2;
		i3 = -i2;
		numSolution = 1;
	}
	else if(isEqual(cf,0) && isEqual(cg,0) && isEqual(ch,0)) {//all three roots are equal and real
		y1 = cubicRoot(cc3) - 1;
		numSolution = 3;
	}
	else {//three real root
		float ci = sqrt(cg*cg/4 -ch);
		float cj = cubicRoot(ci);
		float ck = acos(-cg/(2*ci));
		float cl = -cj;
		float cm = cos(ck/3);
		float cn = sqrt(3.0)*sin(ck/3);
		float cp = -(cc1/3);
		y1 = 2*cj*cm+cp;
		y2 = cl*(cm+cn)+cp;
		y3 = cl*(cm-cn)+cp;
		numSolution = 3;
	}
	if(3 == numSolution) {
		float qp, qq;	//let qq and qp be square root of any two non zero from y1, y2 and y3
		if(isEqual(y1,0)) {
			qp = sqrt(y2);
			qq = sqrt(y3);
		}
		else {
			qp = sqrt(y1);
			qq = sqrt((isEqual(y2,0)?y3:y2));
		}
		float qr = -qg/(8*qq*qp);
		float qs = B/(4*A);
		t1 = qq + qp + qr - qs;
		t2 = qq - qp - qr - qs;
		t3 = -qq + qp - qr - qs;
		t4 = -qq - qp + qr - qs;
		list_intersect.push_back(p+d*t1);
		list_intersect.push_back(p+d*t2);
		list_intersect.push_back(p+d*t3);
		list_intersect.push_back(p+d*t4);
		printf("%.3f, %3f, %3f, %3f\n",t1,t2,t3,t4);
	}
	else if( 1 == numSolution) {
		float qp, qpi, qq, qqi;
		//qp+Iqpi is square root of imagin root y2
		//qq+Iqqi is square root of imagin root y3
		qpi = sqrt((sqrt(y2*y2+i2*i2)-y2)/2);
		qp = i2/(2*qpi);
		qqi = -qpi;//sqrt((sqrt(y3*y3+i3*i3)-y3)/2);
		qq = qp;//i2/(2*qqi);
		float qr = -qg/(8*(qp*qq-qpi*qqi));
		float qs = B/(4*A);
		t1 = qp + qq + qr - qs;
		t2 = -(qp + qq) + qr - qs;
		list_intersect.push_back(p+d*t1);
		list_intersect.push_back(p+d*t2);
		printf("%.3f, %3f\n",t1,t2);
	}

	//transit point back to world coordinate
	for(list<MVertex>::iterator it = list_intersect.begin(); it != list_intersect.end(); it++) {
		(*it) = (*it).scaling(ms.x(),ms.y(),ms.z());
		(*it) = (*it).rotateX(mr.x());
		(*it) = (*it).rotateY(mr.y());
		(*it) = (*it).rotateZ(mr.z());
		(*it) = (*it).translate(mt.x(),mt.y(),mt.z());
		cout<<"intersection: "<<(*it)<<endl;
	}
}